#include "TargetCoverageCheck.hpp"
#include "MiscFunctions.h"

#include <cmath>
#include <stdio.h>
#include <stdexcept>

using namespace idch;
using namespace std;
using namespace cv;

TargetCoverageCheck::TargetCoverageCheck( int imgWidth , int imgHeight ,
                                          double desiredCoverage )
{

    this->desiredCoverage = desiredCoverage;
    this->angleTol = 0.05;
    this->distRelTol = 0.05;
    this->edgeTol = 15;
    this->imgWidth = imgWidth;
    this->imgHeight = imgHeight;
    this->totalCoverage = 0;

    mask = Mat::zeros(imgHeight, imgWidth, CV_8UC1);
}

TargetCoverageCheck::~TargetCoverageCheck() {

}

bool TargetCoverageCheck::addTarget( const CalibrationLocation &target ) {
    if( isTouchingImageEdge(target.depth) || isTouchingImageEdge(target.rgb)) {
        return false;
    }

    if( !isTargetSimilar(target)) {
        return false;
    }

    // update the coverage mask
    addToMask(target.depth.tl);
    addToMask(target.depth.tr);
    addToMask(target.depth.br);
    addToMask(target.depth.bl);

    targets.push_back(target);

    return true;
}

bool TargetCoverageCheck::isImageCovered() {
    return getCoverage() >= desiredCoverage;
}

double TargetCoverageCheck::getCoverage() {
    return static_cast<double>(totalCoverage)/static_cast<double>(imgWidth*imgHeight);
}

void TargetCoverageCheck::drawCoverage( cv::Mat &img ) {

    if( img.channels() != 3 )
        throw std::runtime_error("Expected RGB input image");
    if( img.rows != imgHeight || img.cols != imgWidth )
        throw std::runtime_error("Unexpected input image size");

    uint8_t *rgbRow = (uint8_t *)img.datastart;
    uint8_t *maskRow = (uint8_t *)mask.datastart;

    for( int i = 0; i < imgHeight; i++ ) {

        uint8_t *rgbPtr = rgbRow;
        uint8_t *maskPtr = maskRow;

        for( int j = 0; j < imgWidth; j++ ) {
            if( *maskPtr++ == 0 ) {
                *rgbPtr++ ^= 0xC0;
                *rgbPtr++ = 0xFF;
                *rgbPtr++ ^= 0xC0;
            } else {
                rgbPtr += 3;
            }
        }

        rgbRow += img.step;
        maskRow += mask.step;
    }
}

const std::vector<CalibrationLocation>& TargetCoverageCheck::getTagets() {
    return targets;
}

void TargetCoverageCheck::addToMask( const cv::Point2f &corner ) {
    int r = 30;

    int x0 = (int)corner.x - r;
    int y0 = (int)corner.y - r;
    int x1 = (int)corner.x + r;
    int y1 = (int)corner.y + r;

    if( x0 < 0 ) x0 = 0;
    if( y0 < 0 ) y0 = 0;
    if( x1 > imgWidth ) x1 = imgWidth;
    if( y1 > imgHeight ) y1 = imgHeight;

    for( int y = y0; y < y1; y++ ) {
        uint8_t *ptr = (uint8_t *)mask.datastart + y*mask.step + x0;

        for( int x = x0; x < x1; x++ , ptr++ ) {
            if( *ptr == 0 ) {
                totalCoverage++;
                *ptr = 0xFF;
            }
        }
    }
}

bool TargetCoverageCheck::isTouchingImageEdge( const RectPt &target )
{
    int rightTol = imgWidth-edgeTol;
    int bottomTol = imgHeight-edgeTol;

    if( target.tl.x < edgeTol || target.tl.y < edgeTol )
        return true;
    if( target.tr.x > rightTol || target.tr.y < edgeTol )
        return true;
    if( target.br.x > rightTol || target.br.y > bottomTol )
        return true;
    if( target.bl.x < edgeTol || target.bl.y > bottomTol )
        return true;

    return false;
}

bool TargetCoverageCheck::isTargetSimilar( const CalibrationLocation &target )
{
    double anglesDepth[4];
    double lengthDepth[4];
    double anglesRGB[4];
    double lengthRGB[4];

    // compute a description of the target in relative terms
    computeRelativeDesc(target.depth,anglesDepth,lengthDepth);
    computeRelativeDesc(target.rgb,anglesRGB,lengthRGB);

    // see if the angles are similar
    for( int i = 0; i < 4; i++ ) {
        if( std::abs<double>(anglesDepth[i]-anglesRGB[i]) > angleTol ) {
            return false;
        }
    }

    // similar ratio for edge length
    for( int i = 0; i < 4; i++ ) {

        double diff = std::abs<double>(lengthDepth[i]-lengthRGB[i]);
//        printf(" length diff[%d] = %f\n",i,diff);

        if( std::abs<double>(lengthDepth[i]-lengthRGB[i]) > distRelTol ) {
            return false;
        }
    }

    return true;
}

void TargetCoverageCheck::computeRelativeDesc( const RectPt& target , double *angles , double *length ) {

    const cv::Point2f &tl = target.tl;
    const cv::Point2f &tr = target.tr;
    const cv::Point2f &bl = target.bl;
    const cv::Point2f &br = target.br;

    angles[0] = acuteAngle( tl , tr , bl );
    angles[1] = acuteAngle( tr , tl , br );
    angles[2] = acuteAngle( br , tr , bl );
    angles[3] = acuteAngle( bl , tl , br );

    length[0] = distance<float>( tl.x , tl.y , tr.x , tr.y );
    length[1] = distance<float>( br.x , br.y , tr.x , tr.y );
    length[2] = distance<float>( br.x , br.y , bl.x , bl.y );
    length[3] = distance<float>( tl.x , tl.y , bl.x , bl.y );

    double orig = length[0];
    for( int i = 0; i < 4; i++ ) {
        length[i] /= orig;
    }

//    printf(" angles %4.2f %4.2f %4.2f %4.2f  length  %3.1f %3.1f %3.1f %3.1f\n",
//           angles[0],angles[1],angles[2],angles[3],length[0],length[1],length[2],length[3]);
}


